#include <iostream>
#include<sys/time.h>
#include<xmmintrin.h>
#define N 1000
using namespace std;
float A[N][N];
void display(float A[N][N]) {
    for(int i = 0; i < N; i++) {
        for(int j = 0; j < N; j++) {
            cout<<A[i][j]<<" ";
        }
        cout<<endl;
    }
}//check outcome
void init0(float A[N][N]) {
    cout<<"Initializing ...\n";
    for(int i = 0; i < N; i++) {
        for(int j = 0; j < N; j++) {
            A[i][j] = rand();
        }
    }
}
void init(float A[N][N]) {
    for(int i = 0; i < N; i++) {
        for(int j = 0; j < N; j++) {
            A[i][j] = 0;
        }
        A[i][i] = 1.0;
        for(int j = i + 1; j < N; j++) {
            A[i][j] = rand();
        }
    }
    for(int k = 0; k < N; k++) {
        for(int i = k + 1; i < N; i++) {
            for(int j = 0; j < N; j++) {
                A[i][j] += A[k][j];
            }
        }
    }//initialize
}
void checkAlgn(float A[N][N]) {
    cout<<&A[0][0]<<endl;
}//check alignment
void normal(float A[N][N]) {
    struct timeval h, t;
    checkAlgn(A);
    //Normal
    cout<<"Normal:(N = "<<N<<")\n";
    gettimeofday(&h, NULL);
    for(int k = 0; k < N; k++) {
        for(int j = k + 1; j < N; j++) {
            A[k][j] = A[k][j]/A[k][k];
        }
        A[k][k] = 1.0;
        for(int i = k + 1; i < N; i++) {
            for(int j = k + 1; j < N; j++) {
                A[i][j] = A[i][j] - A[i][k]*A[k][j];
            }
            A[i][k] = 0;
        }
    }
    gettimeofday(&t, NULL);
    cout<<"TIME: "<<(1000*(t.tv_sec - h.tv_sec) + 0.001*(t.tv_usec - h.tv_usec))<<" ms"<<endl;
}
//SSE全部并行（不对齐）
void sse(float A[N][N]) {
    struct timeval h, t;
    checkAlgn(A);
    //SSE
    cout<<"SSE Both:(N = "<<N<<")\n";
    gettimeofday(&h, NULL);
    for(int k = 0; k < N; k++) {
        float* akk = new float[4];
        *akk = *(akk+1) = *(akk+2) = *(akk+3) = A[k][k];
        __m128 vt = _mm_load_ps(akk);//将四个单精度浮点数从内存加载到向量寄存器
        delete[] akk;
        for(int j = k + 1; j + 3 < N; j += 4) {
            __m128 va = _mm_loadu_ps(&A[k][j]);
            va = _mm_div_ps(va, vt);//A[k][j] = A[k][j]/A[k][k];
            _mm_storeu_ps(&A[k][j], va);//储存到内存
        }
        for(int j = N - N%4; j < N; j++) {
            A[k][j] = A[k][j]/A[k][k];
        }//结尾几个元素串行计算
        A[k][k] = 1.0;
        for(int i = k + 1; i < N; i++) {
            float* aik = new float[4];
            *aik = *(aik+1) = *(aik+2) = *(aik+3) = A[i][k];
            __m128 vaik = _mm_load_ps(aik);//将四个单精度浮点数从内存加载到向量寄存器
            delete[] aik;
            for(int j = k + 1; j + 3 < N; j += 4) {
                __m128 vaij = _mm_loadu_ps(&A[i][j]);
                __m128 vakj = _mm_loadu_ps(&A[k][j]);
                __m128 vx = _mm_mul_ps(vaik, vakj);
                vaij = _mm_sub_ps(vaij, vx);//A[i][j] = A[i][j] - A[i][k]*A[k][j];
                _mm_storeu_ps(&A[i][j], vaij);//存储到内存
            }
            for(int j = N - N%4; j < N; j++) {
                A[i][j] = A[i][j] - A[i][k]*A[k][j];
            }//结尾几个元素串行计算
            A[i][k] = 0;
        }
    }
    gettimeofday(&t, NULL);
    cout<<"TIME: "<<(1000*(t.tv_sec - h.tv_sec) + 0.001*(t.tv_usec - h.tv_usec))<<" ms"<<endl;
}
//SSE全部并行（对齐）
void sseAlgn(float A[N][N]) {
    struct timeval h, t;
    checkAlgn(A);
    //SSE
    //cout<<"SSE Both Aligned:(N = "<<N<<")\n";
    gettimeofday(&h, NULL);
    for(int k = 0; k < N; k++) {
        float* akk = new float[4];
        *akk = *(akk+1) = *(akk+2) = *(akk+3) = A[k][k];
        __m128 vt = _mm_load_ps(akk);//将四个单精度浮点数从内存加载到向量寄存器
        delete[] akk;
        for(int j = k + 1; j < k + 4 - k%4; j++) {
            A[k][j] = A[k][j]/A[k][k];
        }//串行计算至对齐
        for(int j = k + 4 - k%4; j + 3 < N; j += 4) {
            __m128 va = _mm_load_ps(&A[k][j]);
            va = _mm_div_ps(va, vt);//A[k][j] = A[k][j]/A[k][k];
            _mm_store_ps(&A[k][j], va);//储存到内存
        }
        for(int j = N - N%4; j < N; j++) {
            A[k][j] = A[k][j]/A[k][k];
        }//结尾几个元素串行计算
        A[k][k] = 1.0;
        for(int i = k + 1; i < N; i++) {
            float* aik = new float[4];
            *aik = *(aik+1) = *(aik+2) = *(aik+3) = A[i][k];
            __m128 vaik = _mm_load_ps(aik);//将四个单精度浮点数从内存加载到向量寄存器
            delete[] aik;
            for(int j = k + 1; j < k + 4 - k%4; j++) {
                A[i][j] = A[i][j] - A[i][k]*A[k][j];
            }
            for(int j = k + 4 - k%4; j + 3 < N; j += 4) {
                __m128 vaij = _mm_load_ps(&A[i][j]);
                __m128 vakj = _mm_load_ps(&A[k][j]);
                __m128 vx = _mm_mul_ps(vaik, vakj);
                vaij = _mm_sub_ps(vaij, vx);//A[i][j] = A[i][j] - A[i][k]*A[k][j];
                _mm_store_ps(&A[i][j], vaij);//存储到内存
            }
            for(int j = N - N%4; j < N; j++) {
                A[i][j] = A[i][j] - A[i][k]*A[k][j];
            }//结尾几个元素串行计算
            A[i][k] = 0;
        }
    }
    gettimeofday(&t, NULL);
    cout<<"TIME: "<<(1000*(t.tv_sec - h.tv_sec) + 0.001*(t.tv_usec - h.tv_usec))<<" ms"<<endl;
}

int main()
{
    /*
    init0(A);
    //display(A);
    int op;
    cout<<"input op:\n-1: Normal\n0: SSE Both\n1: SSE Both Aligned\n";
    cin>>op;
    switch(op) {
    case -1:
        normal(A);
        break;
    case 0:
        sse(A);
        break;
    case 1:
        sseAlgn(A);
        break;
    default:
        cout<<"please input -1, 0 or 1.\n";
    }
    //display(A);
    */
    init0(A);
    //display(A);
    normal(A);
    init0(A);
    sse(A);
    init0(A);
    sseAlgn(A);
    return 0;
}
